home
***
CD-ROM
|
disk
|
FTP
|
other
***
search
/
3D GFX
/
3D GFX.iso
/
amiutils
/
i_l
/
irit5
/
misc_lib
/
genmat.c
< prev
next >
Wrap
C/C++ Source or Header
|
1995-12-30
|
25KB
|
488 lines
/*****************************************************************************
* General matrix manipulation routines. *
* *
* Written by: Gershon Elber Ver 1.0, June 1988 *
*****************************************************************************/
#include <math.h>
#include "irit_sm.h"
#include "genmat.h"
/*****************************************************************************
* DESCRIPTION: M
* Routine to generate a 4*4 unit matrix: M
* *
* PARAMETERS: M
* Mat: Matrix to initialize as a unit matrix. M
* *
* RETURN VALUE: M
* void M
* *
* KEYWORDS: M
* MatGenUnitMat, transformations, unit matrix M
*****************************************************************************/
void MatGenUnitMat(MatrixType Mat)
{
int i, j;
for (i = 0; i < 4; i++)
for (j = 0; j < 4; j++)
if (i == j)
Mat[i][j] = 1.0;
else
Mat[i][j] = 0.0;
}
/*****************************************************************************
* DESCRIPTION: M
* Routine to generate a 4*4 matrix to translate in Tx, Ty, Tz amounts. M
* *
* PARAMETERS: M
* Tx, Ty, Tz: Translational amounts requested. M
* Mat: Matrix to initialize as a translation matrix. M
* *
* RETURN VALUE: M
* void M
* *
* KEYWORDS: M
* MatGenMatTrans, transformations, translation M
*****************************************************************************/
void MatGenMatTrans(RealType Tx, RealType Ty, RealType Tz, MatrixType Mat)
{
MatGenUnitMat(Mat); /* Make it unit matrix. */
Mat[3][0] = Tx;
Mat[3][1] = Ty;
Mat[3][2] = Tz;
}
/*****************************************************************************
* DESCRIPTION: M
* Routine to generate a 4*4 matrix to Scale x, y, z in Sx, Sy, Sz amounts. M
* *
* PARAMETERS: M
* Sx, Sy, Sz: Scaling factors requested. M
* Mat: Matrix to initialize as a scaling matrix. M
* *
* RETURN VALUE: M
* void M
* *
* KEYWORDS: M
* MatGenMatScale, transformations, scaling M
*****************************************************************************/
void MatGenMatScale(RealType Sx, RealType Sy, RealType Sz, MatrixType Mat)
{
MatGenUnitMat(Mat); /* Make it unit matrix. */
Mat[0][0] = Sx;
Mat[1][1] = Sy;
Mat[2][2] = Sz;
}
/*****************************************************************************
* DESCRIPTION: M
* Routine to generate a 4*4 matrix to uniformly scale Scale amount. M
* *
* PARAMETERS: M
* Scale: Uniform scaling factor requested. M
* Mat: Matrix to initialize as a scaling matrix. M
* *
* RETURN VALUE: M
* void M
* *
* KEYWORDS: M
* MatGenMatUnifScale, transformations, scaling M
*****************************************************************************/
void MatGenMatUnifScale(RealType Scale, MatrixType Mat)
{
MatGenMatScale(Scale, Scale, Scale, Mat);
}
/*****************************************************************************
* DESCRIPTION: M
* Routine to generate a 4*4 matrix to Rotate around the X axis by Teta M
* radians. M
* *
* PARAMETERS: M
* Teta: Amount of rotation, in radians. M
* Mat: Matrix to initialize as a rotation matrix. M
* *
* RETURN VALUE: M
* void M
* *
* KEYWORDS: M
* MatGenMatRotX1, transformations, rotation M
*****************************************************************************/
void MatGenMatRotX1(RealType Teta, MatrixType Mat)
{
RealType CTeta, STeta;
CTeta = cos(Teta);
STeta = sin(Teta);
MatGenMatRotX(CTeta, STeta, Mat);
}
/*****************************************************************************
* DESCRIPTION: M
* Routine to generate a 4*4 matrix to Rotate around the X axis by Teta, M
* given the sin and cosine of Teta M
* *
* PARAMETERS: M
* SinTeta, CosTeta: Amount of rotation, given as sine and cosine of Teta. M
* Mat: Matrix to initialize as a rotation matrix. M
* *
* RETURN VALUE: M
* void M
* *
* KEYWORDS: M
* MatGenMatRotX, transformations, rotation M
*****************************************************************************/
void MatGenMatRotX(RealType CosTeta, RealType SinTeta, MatrixType Mat)
{
MatGenUnitMat(Mat); /* Make it unit matrix. */
Mat[1][1] = CosTeta;
Mat[1][2] = SinTeta;
Mat[2][1] = -SinTeta;
Mat[2][2] = CosTeta;
}
/*****************************************************************************
* DESCRIPTION: M
* Routine to generate a 4*4 matrix to Rotate around the Y axis by Teta M
* radians. M
* *
* PARAMETERS: M
* Teta: Amount of rotation, in radians. M
* Mat: Matrix to initialize as a rotation matrix. M
* *
* RETURN VALUE: M
* void M
* *
* KEYWORDS: M
* MatGenMatRotY1, transformations, rotation M
*****************************************************************************/
void MatGenMatRotY1(RealType Teta, MatrixType Mat)
{
RealType CTeta, STeta;
CTeta = cos(Teta);
STeta = sin(Teta);
MatGenMatRotY(CTeta, STeta, Mat);
}
/*****************************************************************************
* DESCRIPTION: M
* Routine to generate a 4*4 matrix to Rotate around the Y axis by Teta, M
* given the sin and cosine of Teta M
* *
* PARAMETERS: M
* SinTeta, CosTeta: Amount of rotation, given as sine and cosine of Teta. M
* Mat: Matrix to initialize as a rotation matrix. M
* *
* RETURN VALUE: M
* void M
* *
* KEYWORDS: M
* MatGenMatRotY, transformations, rotation M
*****************************************************************************/
void MatGenMatRotY(RealType CosTeta, RealType SinTeta, MatrixType Mat)
{
MatGenUnitMat(Mat); /* Make it unit matrix. */
Mat[0][0] = CosTeta;
Mat[0][2] = -SinTeta;
Mat[2][0] = SinTeta;
Mat[2][2] = CosTeta;
}
/*****************************************************************************
* DESCRIPTION: M
* Routine to generate a 4*4 matrix to Rotate around the Z axis by Teta M
* radians. M
* *
* PARAMETERS: M
* Teta: Amount of rotation, in radians. M
* Mat: Matrix to initialize as a rotation matrix. M
* *
* RETURN VALUE: M
* void M
* *
* KEYWORDS: M
* MatGenMatRotZ1, transformations, rotation M
*****************************************************************************/
void MatGenMatRotZ1(RealType Teta, MatrixType Mat)
{
RealType CTeta, STeta;
CTeta = cos(Teta);
STeta = sin(Teta);
MatGenMatRotZ(CTeta, STeta, Mat);
}
/*****************************************************************************
* DESCRIPTION: M
* Routine to generate a 4*4 matrix to Rotate around the Z axis by Teta, M
* given the sin and cosine of Teta M
* *
* PARAMETERS: M
* SinTeta, CosTeta: Amount of rotation, given as sine and cosine of Teta. M
* Mat: Matrix to initialize as a rotation matrix. M
* *
* RETURN VALUE: M
* void M
* *
* KEYWORDS: M
* MatGenMatRotZ, transformations, rotation M
*****************************************************************************/
void MatGenMatRotZ(RealType CosTeta, RealType SinTeta, MatrixType Mat)
{
MatGenUnitMat(Mat); /* Make it unit matrix. */
Mat[0][0] = CosTeta;
Mat[0][1] = SinTeta;
Mat[1][0] = -SinTeta;
Mat[1][1] = CosTeta;
}
/*****************************************************************************
* DESCRIPTION: M
* Routine to multiply 2 4by4 matrices. M
* MatRes may be one of Mat1 or Mat2 - it is only updated in the end. M
* *
* PARAMETERS: M
* MatRes: Result of matrix product. M
* Mat1, Mat2: The two operand of the matrix product. M
* *
* RETURN VALUE: M
* void M
* *
* KEYWORDS: M
* MatMultTwo4by4, transformations, matrix product M
*****************************************************************************/
void MatMultTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2)
{
int i, j, k;
MatrixType MatResTemp;
for (i = 0; i < 4; i++)
for (j = 0; j < 4; j++) {
MatResTemp[i][j] = 0;
for (k = 0; k < 4; k++)
MatResTemp[i][j] += Mat1[i][k] * Mat2[k][j];
}
for (i = 0; i < 4; i++)
for (j = 0; j < 4; j++)
MatRes[i][j] = MatResTemp[i][j];
}
/*****************************************************************************
* DESCRIPTION: M
* Routine to add 2 4by4 matrices. M
* MatRes may be one of Mat1 or Mat2. M
* *
* PARAMETERS: M
* MatRes: Result of matrix addition. M
* Mat1, Mat2: The two operand of the matrix addition. M
* *
* RETURN VALUE: M
* void M
* *
* KEYWORDS: M
* MatAddTwo4by4, transformations, matrix addition M
*****************************************************************************/
void MatAddTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2)
{
int i, j;
for (i = 0; i < 4; i++)
for (j = 0; j < 4; j++)
MatRes[i][j] = Mat1[i][j] + Mat2[i][j];
}
/*****************************************************************************
* DESCRIPTION: M
* Routine to subtract 2 4by4 matrices. M
* MatRes may be one of Mat1 or Mat2. M
* *
* PARAMETERS: M
* MatRes: Result of matrix subtraction. M
* Mat1, Mat2: The two operand of the matrix subtraction. M
* *
* RETURN VALUE: M
* void M
* *
* KEYWORDS: M
* MatSubTwo4by4, transformations, matrix subtraction M
*****************************************************************************/
void MatSubTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2)
{
int i, j;
for (i = 0; i < 4; i++)
for (j = 0; j < 4; j++)
MatRes[i][j] = Mat1[i][j] - Mat2[i][j];
}
/*****************************************************************************
* DESCRIPTION: M
* Routine to scale a 4by4 matrix. M
* MatRes may be Mat. M
* *
* PARAMETERS: M
* MatRes: Result of matrix scaling. M
* Mat: The two operand of the matrix scaling. M
* Scale: Scalar value to multiple matrix with. M
* *
* RETURN VALUE: M
* void M
* *
* KEYWORDS: M
* MatScale4by4, transformations, matrix scaling M
*****************************************************************************/
void MatScale4by4(MatrixType MatRes, MatrixType Mat, RealType *Scale)
{
int i, j;
for (i = 0; i < 4; i++)
for (j = 0; j < 4; j++)
MatRes[i][j] = Mat[i][j] * (*Scale);
}
/*****************************************************************************
* DESCRIPTION: M
* Routine to multiply an XYZ Vector by 4by4 matrix: M
* The Vector has only 3 components (X, Y, Z) and it is assumed that W = 1 M
* VRes may be Vec as it is only updated in the end. M
* *
* PARAMETERS: M
* VRes: Result of vector - matrix product. M
* Vec: Vector to transfrom using Matrix. M
* Mat: Transformation matrix. M
* *
* RETURN VALUE: M
* void M
* *
* KEYWORDS: M
* MatMultVecby4by4, transformations, vector matrix product M
*****************************************************************************/
void MatMultVecby4by4(VectorType VRes, VectorType Vec, MatrixType Mat)
{
int i, j;
RealType
CalcW = Mat[3][3];
VectorType VTemp;
for (i = 0; i < 3; i++) {
VTemp[i] = Mat[3][i]; /* Initiate it with the weight factor. */
for (j = 0; j < 3; j++)
VTemp[i] += Vec[j] * Mat[j][i];
}
for (i = 0; i < 3; i++)
CalcW += Vec[i] * Mat[i][3];
if (CalcW == 0)
CalcW = 1 / INFINITY;
for (i = 0; i < 3; i++)
VRes[i] = VTemp[i] / CalcW;
}
/*****************************************************************************
* DESCRIPTION: M
* Routine to multiply a WXYZ Vector by 4by4 matrix: M
* The Vector has only 4 components (W, X, Y, Z). M
* VRes may be Vec as it is only updated in the end. M
* *
* PARAMETERS: M
* VRes: Result of vector - matrix product. M
* Vec: Vector to transfrom using Matrix. M
* Mat: Transformation matrix. M
* *
* RETURN VALUE: M
* void M
* *
* KEYWORDS: M
* MatMultWVecby4by4, transformations, vector matrix product M
*****************************************************************************/
void MatMultWVecby4by4(RealType VRes[4], RealType Vec[4], MatrixType Mat)
{
int i, j;
RealType VTemp[4];
for (i = 0; i < 4; i++) {
VTemp[i] = 0.0;
for (j = 0; j < 4; j++)
VTemp[i] += Vec[j] * Mat[j][i];
}
for (i = 0; i < 4; i++)
VRes[i] = VTemp[i];
}
/*****************************************************************************
* DESCRIPTION: M
* Routine to compute the INVERSE of a given matrix M which is not modified. M
* The matrix is assumed to be 4 by 4 (transformation matrix). M
* Return TRUE if inverted matrix (InvM) do exists. M
* *
* PARAMETERS: M
* M: Original matrix to invert. M
* InvM: Inverted matrix will be placed here. M
* *
* RETURN VALUE: M
* int: TRUE if inverse exists, FALSE otherwise. M
* *
* KEYWORDS: M
* MatInverseMatrix, transformations, matrix inverse M
*****************************************************************************/
int MatInverseMatrix(MatrixType M, MatrixType InvM)
{
MatrixType A;
int i, j, k;
RealType V;
MAT_COPY(A, M); /* Prepare temporary copy of M in A. */
MatGenUnitMat(InvM); /* Make it unit matrix. */
for (i = 0; i < 4; i++) {
V = A[i][i]; /* Find the new pivot. */
k = i;
for (j = i + 1; j < 4; j++)
if (ABS(A[j][i]) > ABS(V)) {
/* Find maximum on col i, row i+1..n */
V = A[j][i];
k = j;
}
j = k;
if (i != j)
for (k = 0; k < 4; k++) {
SWAP(RealType, A[i][k], A[j][k]);
SWAP(RealType, InvM[i][k], InvM[j][k]);
}
for (j = i + 1; j < 4; j++) { /* Eliminate col i from row i+1..n. */
V = A[j][i] / A[i][i];
for (k = 0; k < 4; k++) {
A[j][k] -= V * A[i][k];
InvM[j][k] -= V * InvM[i][k];
}
}
}
for (i = 3; i >= 0; i--) { /* Back Substitution. */
if (A[i][i] == 0)
return FALSE; /* Error. */
for (j = 0; j < i; j++) { /* Eliminate col i from row 1..i-1. */
V = A[j][i] / A[i][i];
for (k = 0; k < 4; k++) {
/* A[j][k] -= V * A[i][k]; */
InvM[j][k] -= V * InvM[i][k];
}
}
}
for (i = 0; i < 4; i++) /* Normalize the inverse Matrix. */
for (j = 0; j < 4; j++)
InvM[i][j] /= A[i][i];
return TRUE;
}
